// c++
#pragma once
#include <pthread.h>
#include <chrono>
#include <cmath>
#include <iostream>
#include <string>
#include <thread>
#include <vector>

// ROS
#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"
#include "geometry_msgs/msg/point.hpp"
#include "image_transport/image_transport.hpp"
#include "sensor_msgs/msg/image.hpp"

#include "std_msgs/msg/header.hpp"
#include "cv_bridge/cv_bridge.h"
// OpenCv

#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/objdetect/objdetect.hpp>
#include <sensor_msgs/msg/nav_sat_fix.hpp>
// darknet_ros_msgs

#include "yolo_detect_message/msg/bounding_box.hpp"
#include "yolo_detect_message/msg/bounding_boxes.hpp"
#include "yolo_detect_message/msg/object_count.hpp"
#include "yolo_detect_message/msg/traker_box.hpp"
#include "yolo_detect_message/msg/traker_boxs.hpp"
#include "yolo_detect_message/msg/location_traker_box.hpp"
#include "yolo_detect_message/msg/location_traker_boxs.hpp"
#include "yolo_detect_message/srv/service.hpp"
#include "yolo_byte_traker/yolo_tracker.hpp"
#include "yolo_byte_traker/BYTETracker.h"
#include "yolo_detect_message/msg/call_server_status.hpp"
cv::Scalar getRandomColor();
class yolo_byte_traker: public rclcpp::Node
{
private:

    Configuration yolo_config;
    YOLOv10 yolo_traker;
    BYTETracker byte_tracker;
    
    int numClasses_;
    std::vector<std::string> classLabels_;
    rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr detectionImagePublisher_;
    rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr trakerImagePublisher_;
    rclcpp::Publisher<yolo_detect_message::msg::TrakerBoxs>::SharedPtr trakerBoxresult;
    // std::shared_ptr<image_transport::ImageTransport> image_transport_;
    image_transport::Subscriber imageSubscriber_;
    rclcpp::Subscription<yolo_detect_message::msg::CallServerStatus>::SharedPtr service_status_subscriber_;
    rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr cameraSubscriber_;
    rclcpp::CallbackGroup::SharedPtr callback_group_;
    rclcpp::Service<yolo_detect_message::srv::Service>::SharedPtr service_;

    std::mutex service_status_mutex;
    bool service_status =false;
    std::string service_name;
    std::string call_server_topic_name;
    int vehcile_id_;
    std::string namespaces;
    std::string detectionImageTopicName;
    std::string cameraTopicName;
    std::string TrackerImageTopicName;
    std::string trakerBoxTopicName;

    sensor_msgs::msg::NavSatFix uav_gloabal_position;
    std::string gloabal_position_topic;
    rclcpp::Subscription<sensor_msgs::msg::NavSatFix>::SharedPtr global_subscription_;

    std::string location_trakerBoxresult_topic;
    rclcpp::Publisher<yolo_detect_message::msg::LocationTrakerBoxs>::SharedPtr location_trakerBoxresult;
    int image_tag = 0;
    std::string image_path = "/home/nvidia/soft_simulate/src/yolo_byte_traker/source/images/";
public:
    void uav_global_position_callback(const sensor_msgs::msg::NavSatFix::SharedPtr  msg);
    
    void info_topic();
    yolo_byte_traker();
    ~yolo_byte_traker();
    bool readParameters();
    void service_status_callback(const yolo_detect_message::msg::CallServerStatus::SharedPtr msg);
    void init();
    void service_callback(const yolo_detect_message::srv::Service::Request::SharedPtr req,
                        const yolo_detect_message::srv::Service::Response::SharedPtr res);
    void cameraCallback(const std::shared_ptr<const sensor_msgs::msg::Image> msg);
    bool publishDetectionImage(const cv::Mat& detectionImage);
    cv::Mat Draw_Line(const cv::Mat &image, const std::vector<STrack> &detections);
    bool publishTrackerImage(const cv::Mat& detectionImage);
};